#include "ContoursExtractor.h"

void removeDuplicates(QVector<QVector3D>& points, float epsilon = 1e-4f) {
    QVector<QVector3D> unique;
    for (const QVector3D& p : points) {
        bool isDuplicate = false;
        for (const QVector3D& up : unique) {
            if ((p - up).lengthSquared() < epsilon * epsilon) {
                isDuplicate = true;
                break;
            }
        }
        if (!isDuplicate) {
            unique.append(p);
        }
    }
    points = unique;
}

void g0(const Triangle &in_triangle, const QVector3D &in_plane_norm, float in_lambda, Edge &out_edge)
{
    // 平面上的一个点
    QVector3D plane_point = in_lambda * in_plane_norm;

    // 三角形的三个顶点
    auto endpoint1 = std::make_shared<Vertex>();
    endpoint1->pos = in_triangle.v1; endpoint1->norm = in_triangle.normal; endpoint1->color = in_triangle.v1_color;

    auto endpoint2 = std::make_shared<Vertex>();
    endpoint2->pos = in_triangle.v2; endpoint2->norm = in_triangle.normal; endpoint2->color = in_triangle.v2_color;

    auto endpoint3 = std::make_shared<Vertex>();
    endpoint3->pos = in_triangle.v3; endpoint3->norm = in_triangle.normal; endpoint3->color = in_triangle.v3_color;

    // 用于存储交点的临时变量
    QVector<QVector3D> intersections;
    QVector<QVector3D> intersections_norm;
    QVector<QVector4D> intersections_color;
    QVector<QPair<std::shared_ptr<Vertex>, std::shared_ptr<Vertex>>> intersections_endPoints;

    // 在计算交点时，增加对边端点位于平面上的判断
    auto addIntersectionIfOnPlane = [&](const QVector3D& point) {
        if (std::abs(QVector3D::dotProduct(in_plane_norm, point - plane_point)) < 1e-6f) {
            intersections.push_back(point);
        }
    };

    // 检查三角形顶点是否在平面上
    addIntersectionIfOnPlane(endpoint1->pos);
    addIntersectionIfOnPlane(endpoint2->pos);
    addIntersectionIfOnPlane(endpoint3->pos);

    // 计算边 v1-v2 与平面的交点
    QVector3D edge1 = endpoint2->pos - endpoint1->pos;
    float denom1 = QVector3D::dotProduct(in_plane_norm, edge1);
    if (std::fabs(denom1) > std::numeric_limits<float>::epsilon()) {
        float t1 = QVector3D::dotProduct(in_plane_norm, plane_point - endpoint1->pos) / denom1;
        if (t1 >= 0.0f && t1 <= 1.0f) {
            intersections.push_back(endpoint1->pos + t1 * edge1);
            intersections_color.push_back(endpoint1->color + t1 * (endpoint2->color - endpoint1->color));
            intersections_norm.push_back(endpoint1->norm);
            intersections_endPoints.push_back(qMakePair(endpoint2, endpoint1));
        }
    }

    // 计算边 v2-v3 与平面的交点
    QVector3D edge2 = endpoint3->pos - endpoint2->pos;;
    float denom2 = QVector3D::dotProduct(in_plane_norm, edge2);
    if (std::fabs(denom2) > std::numeric_limits<float>::epsilon()) {
        float t2 = QVector3D::dotProduct(in_plane_norm, plane_point - endpoint2->pos) / denom2;
        if (t2 >= 0.0f && t2 <= 1.0f) {
            intersections.push_back(endpoint2->pos + t2 * edge2);
            intersections_color.push_back(endpoint2->color + t2 * (endpoint3->color - endpoint2->color));
            intersections_norm.push_back(endpoint2->norm);
            intersections_endPoints.push_back(qMakePair(endpoint3, endpoint2));
        }
    }

    // 计算边 v3-v1 与平面的交点
    QVector3D edge3 = endpoint1->pos - endpoint3->pos;
    float denom3 = QVector3D::dotProduct(in_plane_norm, edge3);
    if (std::fabs(denom3) > std::numeric_limits<float>::epsilon()) {
        float t3 = QVector3D::dotProduct(in_plane_norm, plane_point - endpoint3->pos) / denom3;
        if (t3 >= 0.0f && t3 <= 1.0f) {
            intersections.push_back(endpoint3->pos + t3 * edge3);
            intersections_color.push_back(endpoint3->color + t3 * (endpoint1->color - endpoint3->color));
            intersections_norm.push_back(endpoint3->norm);
            intersections_endPoints.push_back(qMakePair(endpoint1, endpoint3));
        }
    }

    // 去重
    removeDuplicates(intersections);

    // 如果找到了两个交点并且不是同一个（来自交一个三角面片的顶点），则输出交线
    if (intersections.size() == 2)
    {
        out_edge.v1.pos = intersections[0];
        out_edge.v1.norm = intersections_norm[0];
        out_edge.v1.color = intersections_color[0];
        out_edge.v1.sptr_endPointa = intersections_endPoints[0].first;
        out_edge.v1.sptr_endPointb = intersections_endPoints[0].second;
        out_edge.v2.pos = intersections[1];
        out_edge.v2.norm = intersections_norm[1];
        out_edge.v2.color = intersections_color[1];
        out_edge.norm = in_plane_norm;
        out_edge.v2.sptr_endPointa = intersections_endPoints[1].first;
        out_edge.v2.sptr_endPointb = intersections_endPoints[1].second;
    }
    else
    {
        // 如果没有交点或交点不足两个，输出无效边
        out_edge.v1.pos = QVector3D();
        out_edge.v2.pos = QVector3D();
        out_edge.norm = QVector3D();
    }
}

QVector<Triangle> h0(const QVector<Triangle> &model_triangles, const QVector3D &plane_norm, float lambda)
{
    QVector<Triangle> intersecting_triangles; // 存储与平面相交的三角面片

    // 计算平面上的点 P0
    QVector3D P0 = lambda * plane_norm;

    // 遍历所有三角面片
    for (const Triangle& triangle : model_triangles) {
        // 获取三角面片的三个顶点
        QVector3D v1 = triangle.v1;
        QVector3D v2 = triangle.v2;
        QVector3D v3 = triangle.v3;

        // 计算每个顶点与平面的位置关系
        float d1 = QVector3D::dotProduct(plane_norm, v1 - P0);
        float d2 = QVector3D::dotProduct(plane_norm, v2 - P0);
        float d3 = QVector3D::dotProduct(plane_norm, v3 - P0);

        // 判断三角面片是否与平面相交
        if ((d1 * d2 <= 0) || (d1 * d3 <= 0) || (d2 * d3 <= 0)) {
            // 如果三个顶点不全在平面的同一侧，则三角面片与平面相交
            intersecting_triangles.append(triangle);
        }
    }

    return intersecting_triangles;
}

void h1(const QVector<Triangle>& in_model_triangles, const QVector3D& in_plane_norm, float in_lambda, QVector<Edge>& links)
{
    // 清空输出邻接关系
    links.clear();

    // 遍历所有三角面片
    for (const Triangle& mesh : in_model_triangles)
    {
        Edge edge;
        g0(mesh, in_plane_norm, in_lambda, edge); // 计算交线

        // 如果交线有效（两个端点有效）
        if (edge.v1.pos != QVector3D() && edge.v2.pos != QVector3D() && edge.norm != QVector3D())
            links.append(edge);
    }
}

void h2(const QVector<Edge>& links, QVector<QVector<Vertex>>& contours)
{
    QSet<Edge> processedEdges;
    const float eps = 1e-5f;  // 调整此容差值以适应您的模型精度

    for (const Edge& edge : links)
    {
        if (processedEdges.contains(edge)) continue;

        QVector<Vertex> contour;
        QQueue<Edge> queue;
        queue.enqueue(edge);
        processedEdges.insert(edge);

        // 初始轮廓：直接添加当前边的两个顶点
        contour.push_back(edge.v1);
        contour.push_back(edge.v2);

        while (!queue.isEmpty()) {
            Edge currentEdge = queue.dequeue();

            // 查找 currentEdge.v1 和 v2 是否能连接到 contour 的任一端
            auto findClosestContourVertex = [&](const Vertex& v) -> int {
                if (contour.isEmpty()) return -1;
                // 检查是否已经存在（避免重复）
                for (int i = 0; i < contour.size(); ++i) {
                    if (v == contour[i]) return i;
                }
                // 如果 `==` 失败，但距离很近，仍然认为可以连接
                float minDist = (v.pos - contour.front().pos).lengthSquared();
                int closestIdx = 0;
                for (int i = 1; i < contour.size(); ++i) {
                    float dist = (v.pos - contour[i].pos).lengthSquared();
                    if (dist < minDist) {
                        minDist = dist;
                        closestIdx = i;
                    }
                }
                return (minDist <= eps * eps) ? closestIdx : -1;
            };

            int v1Idx = findClosestContourVertex(currentEdge.v1);
            int v2Idx = findClosestContourVertex(currentEdge.v2);

            // 动态决定如何连接（优先连接更近的顶点）
            if (v1Idx != -1 || v2Idx != -1) {
                if (v1Idx == 0 && v2Idx == -1) {
                    // v1 匹配 contour 前端 → 尝试把 v2 加入前端
                    contour.push_front(currentEdge.v2);
                } else if (v1Idx == contour.size() - 1 && v2Idx == -1) {
                    // v1 匹配 contour 末端 → 尝试把 v2 加入末端
                    contour.push_back(currentEdge.v2);
                } else if (v2Idx == 0 && v1Idx == -1) {
                    // v2 匹配 contour 前端 → 尝试把 v1 加入前端
                    contour.push_front(currentEdge.v1);
                } else if (v2Idx == contour.size() - 1 && v1Idx == -1) {
                    // v2 匹配 contour 末端 → 尝试把 v1 加入末端
                    contour.push_back(currentEdge.v1);
                }
                // 其他情况（如两个顶点都匹配）暂不处理，避免复杂逻辑
            }

            // 查找邻接边（仍然使用 operator==，但放宽匹配条件）
            for (const Edge& nextEdge : links) {
                if (processedEdges.contains(nextEdge)) continue;

                bool isAdjacent = false;
                // 检查 nextEdge 是否与 contour 的任一端点匹配（使用 operator==）
                if (nextEdge.v1 == contour.front() || nextEdge.v2 == contour.front() ||
                    nextEdge.v1 == contour.back()  || nextEdge.v2 == contour.back()) {
                    isAdjacent = true;
                }

                if (isAdjacent) {
                    queue.enqueue(nextEdge);
                    processedEdges.insert(nextEdge);
                }
            }
        }

        // 检查轮廓是否应该闭合（即使首尾顶点不完全相等）
        if (contour.size() >= 3 &&
            (contour.front().pos - contour.back().pos).lengthSquared() <= eps * eps) {
            contour.push_back(contour.front());  // 强制闭合
        }

        if (contour.size() >= 2) {
            contours.append(contour);
        }
    }

    // 按轮廓大小排序
    std::sort(contours.begin(), contours.end(),
              [](const QVector<Vertex>& a, const QVector<Vertex>& b) {
                  return a.size() > b.size();
              });
}
